#include "nav2_util/node_utils.hpp"
#include <cmath>
#include <memory>
#include <string>
#include <fstream>
#include "nav2_core/exceptions.hpp"
#include "planner_pkg/global_dijkstra_planner.hpp"

namespace planner_pkg
{

    GlobalDijkstraPlanner::GlobalDijkstraPlanner()
        : tf_(nullptr), costmap_(nullptr)
    {
    }

    GlobalDijkstraPlanner::~GlobalDijkstraPlanner()
    {
        RCLCPP_INFO(
            logger_, "Destroying plugin %s of type GlobalDijkstraPlanner",
            name_.c_str());
    }
    void GlobalDijkstraPlanner::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name,
        std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
    {
        tf_ = tf;
        node_ = parent.lock();
        name_ = name;
        costmap_ = costmap_ros->getCostmap();
        global_frame_ = costmap_ros->getGlobalFrameID();

        clock_ = node_->get_clock();
        logger_ = node_->get_logger();
        // 参数初始化
        nav2_util::declare_parameter_if_not_declared(node_, name + ".tolerance", rclcpp::ParameterValue(0.5));
        node_->get_parameter(name_ + ".tolerance", tolerance_);
        node_->get_parameter(name_ + ".allow_unknown", allow_unknown_);
        // 创建导航规划
        planner_ = std::make_unique<Dijkstra>(
            costmap_->getSizeInCellsX(),
            costmap_->getSizeInCellsY());
    }

    void GlobalDijkstraPlanner::cleanup()
    {
        RCLCPP_INFO(logger_, "正在清理类型为 GlobalDijkstraPlanner 的插件 %s",
                    name_.c_str());
        planner_.reset();
    }

    void GlobalDijkstraPlanner::activate()
    {
        RCLCPP_INFO(logger_, "正在激活类型为 GlobalDijkstraPlanner 的插件 %s",
                    name_.c_str());
    }

    void GlobalDijkstraPlanner::deactivate()
    {
        RCLCPP_INFO(logger_, "正在停用类型为 GlobalDijkstraPlanner 的插件 %s",
                    name_.c_str());
    }

    nav_msgs::msg::Path
    GlobalDijkstraPlanner::createPlan(const geometry_msgs::msg::PoseStamped &start,
                                      const geometry_msgs::msg::PoseStamped &goal)
    {
        nav_msgs::msg::Path global_path;
        // Corner case of the start(x,y) = goal(x,y)
        if (start.pose.position.x == goal.pose.position.x &&
            start.pose.position.y == goal.pose.position.y)
        {
            RCLCPP_WARN(logger_, "规划器失败 createPlan start(x,y) = goal(x,y)");
            return global_path;
        }

        //  使用简单的 A* 算法规划路径
        if (!makePlan(start.pose, goal.pose, global_path))
        {
            RCLCPP_WARN(logger_, "规划全局路径失败");
            throw nav2_core::PlannerException("规划全局路径失败");
        }
        // 使用 costmap 检查该条路径是否经过障碍物
        for (geometry_msgs::msg::PoseStamped pose : global_path.poses)
        {
            unsigned int mx, my; // 将点的坐标转换为栅格坐标
            if (costmap_->worldToMap(pose.pose.position.x, pose.pose.position.y, mx, my))
            {
                unsigned char cost = costmap_->getCost(mx, my); // 获取对应栅格的代价值
                // 如果存在致命障碍物则抛出异常
                if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
                {
                    RCLCPP_WARN(logger_, "在(%f,%f)检测到致命障碍物，规划失败。", pose.pose.position.x, pose.pose.position.y);
                    throw nav2_core::PlannerException(
                        "无法创建目标规划: " + std::to_string(goal.pose.position.x) + "," + std::to_string(goal.pose.position.y));
                }
            }
        }
        return global_path;
    }
    /**
     * 从 start 到 goal 的路径，要求过一系列的点，这些点 pose_ 上的点，
     */
    bool GlobalDijkstraPlanner::makePlan(
        const geometry_msgs::msg::Pose &start,
        const geometry_msgs::msg::Pose &goal,
        nav_msgs::msg::Path &plan)
    {
        plan.poses.clear();
        plan.header.frame_id = global_frame_;
        plan.header.stamp = clock_->now();
        // if (poses_.empty())
        // {
        //     RCLCPP_INFO(logger_, "pose_ is empty,无法生成路径");
        //     return false;
        // }
        double wx_start = start.position.x;
        double wy_start = start.position.y;
        unsigned int mx_start, my_start;
        if (!costmap_->worldToMap(wx_start, wy_start, mx_start, my_start))
        {
            RCLCPP_WARN(logger_, "无法规划路径，机器人起始位置不在全局代价地图上。");
            return false;
        }
        // 将起始点设置为可通行
        costmap_->setCost(mx_start, my_start, nav2_costmap_2d::FREE_SPACE);
        planner_->setMapSize(
            costmap_->getSizeInCellsX(),
            costmap_->getSizeInCellsY());
        planner_->setCostmap(costmap_->getCharMap());
        // 起始点
        int map_start[2];
        map_start[0] = mx_start;
        map_start[1] = my_start;
        // 目标点
        double wx_goal = goal.position.x;
        double wy_goal = goal.position.y;
        unsigned int mx_goal, my_goal;
        if (!costmap_->worldToMap(wx_goal, wy_goal, mx_goal, my_goal))
        {
            RCLCPP_WARN(logger_, "无法规划路径，目标点不在全局代价地图上。");
            return false;
        }
        // 检测目标点是否为致命障碍物
        unsigned char cost = costmap_->getCost(mx_goal, my_goal); // 获取对应栅格的代价值
        // 如果存在致命障碍物则抛出异常
        if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
        {
            RCLCPP_WARN(logger_, "在目标点(%f,%f)检测到致命障碍物，规划失败。", goal.position.x, goal.position.y);
            return false;
        }
        int map_goal[2];
        map_goal[0] = mx_goal;
        map_goal[1] = my_goal;
        planner_->setStart(map_start);
        planner_->setGoal(map_goal);
        // 使用 A* 规划
        auto start_time = std::chrono::high_resolution_clock::now();
        std::vector<std::shared_ptr<PointNode>> node_list = planner_->searchPath();
        // 耗时 Path found
        auto end_time = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
        RCLCPP_INFO(logger_, "Path found in %ld us", duration.count());

        // 将节点生成路径
        for (auto node : node_list)
        {
            double world_x, world_y; // 将栅格坐标转换为世界坐标
            costmap_->mapToWorld(node->x, node->y, world_x, world_y);
            // 生成pose
            geometry_msgs::msg::PoseStamped pose;
            pose.pose.position.x = world_x;
            pose.pose.position.y = world_y;
            pose.pose.position.z = 0.0;
            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 1.0;
            plan.poses.push_back(pose);
        }
        //----------------------------------------------
        smoothApproachToGoal(goal, plan);
        return !plan.poses.empty();
    }

    void
    GlobalDijkstraPlanner::smoothApproachToGoal(
        const geometry_msgs::msg::Pose &goal,
        nav_msgs::msg::Path &plan)
    {
        // Replace the last pose of the computed path if it's actually further away
        // to the second to last pose than the goal pose.
        if (plan.poses.size() >= 2)
        {
            auto second_to_last_pose = plan.poses.end()[-2];
            auto last_pose = plan.poses.back();
            if (
                squared_distance(last_pose.pose, second_to_last_pose.pose) >
                squared_distance(goal, second_to_last_pose.pose))
            {
                plan.poses.back().pose = goal;
                return;
            }
        }
        geometry_msgs::msg::PoseStamped goal_copy;
        goal_copy.pose = goal;
        plan.poses.push_back(goal_copy);
    }

} // namespace planner_pkg

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(planner_pkg::GlobalDijkstraPlanner,
                       nav2_core::GlobalPlanner)